// UltrasonicSensor.java
// Most of this class taken from the leJOS library (lejos.sourceforge.net),
// with thanks to the author.

/*
This software is part of the NxtJLib library.
It is Open Source Free Software, so you may
- run the code for any purpose
- study how the code works and adapt it to your needs
- integrate all or parts of the code in your own programs
- redistribute copies of the code
- improve the code and release your improvements to the public
However the use of the code is entirely your responsibility.
*/

package ch.aplu.nxt;

import ch.aplu.nxt.platform.*;

/**
 * Class that represents an ultrasonic sensor.
 * Most of the code and the documentation
 * taken from the leJOS library (lejos.sourceforge.net, with thanks to the author.
 */
public class UltrasonicSensor extends I2CSensor
{
  private interface SensorState // Simulate enum for J2ME compatibility
  {
    int NEAR = 0;
    int FAR = 1;
  }

// -------------- Inner class UltrasonicSensorThread ------------
  private class UltrasonicSensorThread extends NxtThread
  {
    private volatile boolean isRunning = false;

    private UltrasonicSensorThread()
    {
      if (NxtRobot.getDebugLevel() >= DEBUG_LEVEL_LOW)
        DebugConsole.show("DEBUG: UltrasonicSensorThread created (port: " +
          getPortLabel() + ")");
    }

    public void run()
    {
      if (NxtRobot.getDebugLevel() >= DEBUG_LEVEL_LOW)
        DebugConsole.show("DEBUG: UltrasonicSensorThread " +
          Thread.currentThread().getName() +
          " started (port: " + getPortLabel() + ")");

      isRunning = true;
      while (isRunning)
      {
        if (ultrasonicListener != null)
        {
          Tools.delay(pollDelay);
          int level = getLevel();
          if (state == SensorState.NEAR && level > triggerLevel)
          {
            if (inCallback)
            {
              if (NxtRobot.getDebugLevel() >= DEBUG_LEVEL_LOW)
                DebugConsole.show("DEBUG: Ultrasonic event 'far' (port: " +
                  getPortLabel() + ") -------- rejected: Other callback underway!");
            }
            else
            {  
              inCallback = true;
              if (NxtRobot.getDebugLevel() >= DEBUG_LEVEL_LOW)
                DebugConsole.show("DEBUG: Ultrasonic event 'far' (port: " +
                  getPortLabel() + ")");
              ultrasonicListener.far(getPort(), level);
              state = SensorState.FAR;
              inCallback = false;
            }
          }
          if (state == SensorState.FAR && level <= triggerLevel)
          {
            if (inCallback)
            {
              if (NxtRobot.getDebugLevel() >= DEBUG_LEVEL_LOW)
                DebugConsole.show("DEBUG: Ultrasonci event 'near' (port: " +
                  getPortLabel() + ") -------- rejected: Other callback underway!");
            }
            else
            {  
              inCallback = true;
              if (NxtRobot.getDebugLevel() >= DEBUG_LEVEL_LOW)
                DebugConsole.show("DEBUG: Ultrasonic event 'near' (port: " +
                  getPortLabel() + ")");
              ultrasonicListener.near(getPort(), level);
              state = SensorState.NEAR;
              inCallback = false;
            }
          }
        }
      }
    }

    private void stopThread()
    {
      isRunning = false;
      try
      {
        joinX(500);
      }
      catch (InterruptedException ex)
      {
      }
      if (NxtRobot.getDebugLevel() >= DEBUG_LEVEL_LOW)
        if (isAlive())
          DebugConsole.show("DEBUG: UltrasonicSendorThread stopping failed (port: " +
            getPortLabel() + ")");
        else
          DebugConsole.show("DEBUG: UltrasonicSensorThread successfully stopped (port: " +
            getPortLabel() + ")");
    }
  }
// -------------- End of inner classes -----------------------

  private volatile static boolean inCallback = false;
  private UltrasonicListener ultrasonicListener = null;
  private UltrasonicSensorThread ust;
  private int state = SensorState.NEAR;
  private int triggerLevel;
  private int pollDelay;
  private static final byte FACTORY_ZERO = 0x11;
  private static final byte FACTORY_SCALE_FACTOR = 0x12;
  private static final byte FACTORY_SCALE_DIVISOR = 0x13;
  private static final byte CONTINUOUS_MEASUREMENT_INTERVAL = 0x40;
  private static final byte COMMAND_STATE = 0x41; // Command or reply length = 1
  private static final byte BYTE0 = 0x42;
  private static final byte BYTE1 = 0x43;
  private static final byte BYTE2 = 0x44;
  private static final byte BYTE3 = 0x45;
  private static final byte BYTE4 = 0x46;
  private static final byte BYTE5 = 0x47;
  private static final byte BYTE6 = 0x48;
  private static final byte BYTE7 = 0x49;
  private static final byte ACTUAL_ZERO = 0x50;
  private static final byte ACTUAL_SCALE_FACTOR = 0x51;
  private static final byte ACTUAL_SCALE_DIVISOR = 0x52;
  /**
   * Turn off sensor.
   */
  public static byte OFF = 0x00;
  /**
   * Sensor sends out one ping and records result.
   */
  public static byte SINGLE_SHOT = 0x01;
  /**
   * Sensor making continuous readings at set intervals.
   */
  public static byte CONTINUOUS_MEASUREMENT = 0x02;
  /**
   * Puts the sensor in passive mode to record pings that might come from
   * other sonar sensors.
   */
  public static byte EVENT_CAPTURE = 0x03;
  /**
   * Resets the sensor.
   */
  public static byte REQUEST_WARM_RESET = 0x04;

  /**
   * Creates a sensor instance connected to the given port.
   * @param port the port where the sensor is plugged-in
   */
  public UltrasonicSensor(SensorPort port)
  {
    super(port, LOWSPEED_9V);
    ust = new UltrasonicSensorThread();
    NxtProperties props = NxtRobot.getProperties();
    pollDelay = props.getIntValue("UltrasonicSensorPollDelay");
  }

  /**
   * Creates a sensor instance connected to port S1.
   */
  public UltrasonicSensor()
  {
    this(SensorPort.S1);
  }

  /**
   * Sets the state for the sensor. e.g. off, single pulse, continuous, passive, warm reset.
   * @param modeEnumeration e.g. OFF, SINGLE_SHOT, CONTINUOUS_MEASUREMENT, EVENT_CAPTURE, REQUEST_WARM_RESET
   */
  public void setSensorMode(byte modeEnumeration)
  {
    checkConnect();
    sendData(COMMAND_STATE, modeEnumeration);
  }

  protected void cleanup()
  {
    if (NxtRobot.getDebugLevel() >= DEBUG_LEVEL_MEDIUM)
      DebugConsole.show("DEBUG: Ultrasonic.cleanup() called (Port: " +
        getPortLabel() + ")");
    if (ust != null)
      ust.stopThread();
  }

  private int getDistance(boolean check)
  {
    if (check)
      checkConnect();
    setSensorMode(SINGLE_SHOT);
    byte[] val = getData(BYTE0, 1);
    return 0xFF & val[0]; // Convert signed byte to unsigned (positive only)
  }

  private int getLevel()
  {
    if (robot == null || !robot.isConnected())
      return -1;
    return getDistance(false);
  }

  /**
   * Polls the sensor.
   * @return the current distance to the closest object in cm
   */
  public int getDistance()
  {
    Tools.delay(10);
    return getDistance(true);
  }

  /**
   * The ultrasonic sensor is capable of sending a ping and recording up to 8 echoes.
   * A SINGLE_SHOT command is used, rather than continuous pings.
   * This method returns an array of up to 8 distances picked up by the UltrasonicSensor
   * sensor.
   *
   * @return eight distance values, mostly will be 0.
   */
  public byte[] getDistances()
  {
    checkConnect();
    setSensorMode(SINGLE_SHOT);
    return getData(BYTE0, 8);
  }

  /**
   * Registers the given ultrasonic listener for the given trigger level.
   * @param ultrasonicListener the UltrasonicListener to become registered.
   * @param triggerLevel the trigger level where the callback is triggered
   */
  public void addUltrasonicListener(UltrasonicListener ultrasonicListener, int triggerLevel)
  {
    this.ultrasonicListener = ultrasonicListener;
    this.triggerLevel = triggerLevel;
    if (!ust.isAlive())
      startUltrasonicThread();
  }

  /**
   * Registers the given ultrasonic listener with default trigger level 20.
   * @param ultrasonicListener the UltrasonicListener to become registered.
   */
  public void addUltrasonicListener(UltrasonicListener ultrasonicListener)
  {
    addUltrasonicListener(ultrasonicListener, 20);
  }

  /**
   * Sets a new trigger level and returns the previous one.
   * @param triggerLevel the new trigger level
   * @return the previous trigger level
   */
  public int setTriggerLevel(int triggerLevel)
  {
    int oldLevel = this.triggerLevel;
    this.triggerLevel = triggerLevel;
    return oldLevel;
  }

  protected void startUltrasonicThread()
  {
    ust.start();
  }

  protected void stopUltrasonicThread()
  {
    if (ust.isAlive())
      ust.stopThread();
  }


  /**
   * Returns the time interval between measurements in continuous mode.
   * @return the time 0 .. 255 in ms
   */
  public int getMeasurementInterval()
  {
    checkConnect();
    byte val = getData(CONTINUOUS_MEASUREMENT_INTERVAL, 1)[0];
    return (0xFF & val);
  }

  /**
   * Sets the scale factor, producing either cm's, inches, or some other unit.
   * @param scale a number 0 .. 255
   */
  public void setScaleFactor(byte scale)
  {
    checkConnect();
    sendData(ACTUAL_SCALE_FACTOR, scale);
  }

  private void setZero(byte zeroPoint)
  {
    sendData(ACTUAL_ZERO, zeroPoint);
  }

  private void setScaleDivisor(byte divisor)
  {
    sendData(ACTUAL_SCALE_DIVISOR, divisor);
  }

  /**
   * Calibrates the sensor.
   * If your sensor readings seem off, you can calibrate the sensor
   * to make it different from factory settings.
   * @param scaleFactor Raw reading is multiplied by this number. 1 = factory setting
   * @param scaleDivisor Raw reading divides by this number. 14 = factory setting
   * @param zero e.g. If you find the real distance is 10 cm and the sensor
   *        says 12 cm you can adjust the zero. 0 = factory setting
   */
  public void calibrate(byte scaleFactor, byte scaleDivisor, byte zero)
  {
    checkConnect();
    setScaleFactor(scaleFactor);
    setScaleDivisor(scaleDivisor);
    setZero(zero);
  }

  /**
   * Gets the scale factor, which produces either cm's, inches, or some other unit.
   * @return the scale factor 0 .. 255
   */
  public int getScaleFactor()
  {
    checkConnect();
    byte[] val = getData(ACTUAL_SCALE_FACTOR, 1);
    return (0xFF & val[0]);
  }

  /**
   * Gets the default scale factor, which produces either cm's, inches, or some other unit.
   * @return the scale factor 0 .. 255
   */
  public int getFactoryScaleFactor()
  {
    checkConnect();
    byte[] val = getData(FACTORY_SCALE_FACTOR, 1);
    return (0xFF & val[0]);
  }

  /**
   * Gets the default scale divisor, which produces either cm's, inches, or some other unit.
   * @return the scale divisor 0 .. 255
   */
  public int getFactoryScaleDivisor()
  {
    checkConnect();
    byte[] val = getData(FACTORY_SCALE_DIVISOR, 1);
    return (0xFF & val[0]);
  }

  /**
   * Gets the default "zero" point, which should be 0.
   * @return the zero point
   */
  public int getFactoryZero()
  {
    checkConnect();
    byte[] val = getData(FACTORY_ZERO, 1);
    return (0xFF & val[0]);
  }

  /**
   * Sets the time interval between measurements in continuous mode.
   * @param interval the time interval in ms (0 .. 255)
   */
  public void setMeasurementInterval(byte interval)
  {
    checkConnect();
    sendData(CONTINUOUS_MEASUREMENT_INTERVAL, interval);
  }

  private void checkConnect()
  {
    if (robot == null)
      new ShowError("UltrasonicSensor (port: " + getPortLabel() +
        ") is not a part of the NxtRobot.\n" +
        "Call addPart() to assemble it.");
    if (!robot.connect())
      PlatformTools.putSleep();
  }
}
